#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     IRseeker,       sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     LeftMotor,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     RightMotor,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     launcher,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     flag,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_1,     LeftMotor2,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     RightMotor2,   tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    Flipper,              tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.C";
int IRvalue;
int distancetoIR;
int isbaskettoright;


task main()
{
waitForStart();

isbaskettoright = 0;

//if it isn't working did you:
//1)check that motors have enough power not to stall
//2)check that IR beacons are turned on
//3)check that battery is plugged in
//4)check that all sensors are plugged in
//5)check that IR seeker is rotated correctly

//reset motors and encoders
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;



while (isbaskettoright == 0)
{
	if (SensorValue(IRseeker) == 4)
	{
		isbaskettoright = 1;
	}
	motor[LeftMotor]= -30;
	motor[LeftMotor2] = -30;
	motor[RightMotor] = -30;
	motor[RightMotor2] = -30;
	IRvalue = SensorValue(IRseeker);
}
if (nMotorEncoder[LeftMotor] > -1440)
{
	while (nMotorEncoder[LeftMotor] > -800)
	{
		motor[LeftMotor]= -30;
		motor[LeftMotor2] = -30;
		motor[RightMotor] = -30;
		motor[RightMotor2] = -30;
	}
}

wait1Msec(100);

//set distance to IR as the distance that the encoder treaveled
distancetoIR = nMotorEncoder(LeftMotor);


	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;
	//while (nMotorEncoder[LeftMotor] < 1)
	//{
	//motor[LeftMotor]= 30;
	//motor[LeftMotor2] = 30;
	//motor[RightMotor] = 30;
	//motor[RightMotor2] = 30;
	//}


	//set distance to IR as the distance that the encoder treaveled
distancetoIR = distancetoIR + nMotorEncoder(LeftMotor);

//reset motors and encoderz
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;

	wait1Msec(500);

	servo[Flipper] = 224;

	wait1Msec(500);


//adjust variable so that distance is reversed and it is slightly smaller to leave room for turning
	distancetoIR = distancetoIR * - 1;
	distancetoIR = distancetoIR + 500;

	//reset motors and encoderz
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;
	//back up the robot the same distance it came
	while (nMotorEncoder[LeftMotor] < distancetoIR)
	{
	motor[LeftMotor]= 30;
	motor[LeftMotor2] = 30;
	motor[RightMotor] = 30;
	motor[RightMotor2] = 30;
	}

	//reset motors and encoders
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;


	wait10Msec(10);

	while (nMotorEncoder[LeftMotor] < 1840)
	{
	motor[RightMotor] = - 40;
	motor[RightMotor2] = - 40;
	motor[LeftMotor] = 40;
	motor[LeftMotor2] = 40;
}

	//reset motors and encoders
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;

	while (nMotorEncoder[LeftMotor] > -3000)
	{
		motor[LeftMotor]= -40;
		motor[LeftMotor2] = -40;
		motor[RightMotor] = -40;
		motor[RightMotor2] = -40;
	}

	//reset motors and encoders
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;

	while (nMotorEncoder[LeftMotor] < 1840)
	{
	motor[RightMotor] = - 40;
	motor[RightMotor2] = - 40;
	motor[LeftMotor] = 40;
	motor[LeftMotor2] = 40;
}

	//reset motors and encoders
	motor[LeftMotor]= 0;
	motor[LeftMotor2] = 0;
	motor[RightMotor] = 0;
	motor[RightMotor2] = 0;
	nMotorEncoder[LeftMotor] = 0;
	nMotorEncoder[RightMotor] = 0;
	nMotorEncoder[LeftMotor2] = 0;
	nMotorEncoder[RightMotor2] = 0;

	while (nMotorEncoder[LeftMotor] < 5000)
	{
	motor[RightMotor] = 100;
	motor[RightMotor2] = 100;
	motor[LeftMotor] = 100;
	motor[LeftMotor2] = 100;
	}
}
